///<reference path="../rpos.d.ts"/>

import { Stream } from "stream";
import { v4l2ctl } from "./v4l2ctl";
import net = require('net');
import { setImmediate } from "timers";
import events = require("events");

/*
PTZDriver for RPOS (Raspberry Pi ONVIF Server)
(c) 2016, 2017, 2018, 2021 Roger Hardiman
(c) 2018 Casper Meijn
MIT License

This code processes the ONVIF Pan/Tilt/Zoom messages and supports several Pan/Tilt devices.

Tenx USB Missile Launcher Support
  Opens the Tenx USB Missile Launcher USB IDs 0x1130 0x0202
  ONVIF Pan/Tilt turns the missile launcher
  ONVIF GotoPreset fires a foam missile (any preset will work)
  Config settings
      PTZDriver: "tenx"

Pelco D PTZ Telemetry Support
  Opens a serial port, or TCP network connection and sends Pelco D commands including
  Pan, Tilt, Zoom and Preset commands. ONVIF Home Position is mapped to Pelco Preset 1.
  The camera address can be specified.
  Config settings
      PTZDriver: "pelcod",
      PTZCameraAddress: 1
      PTZOutput: SEE BELOW

Sony Visca PTZ Telemetry Support
  Opens a serial port, or TCP network connection and sends VISCA commands including
  Pan, Tilt, Zoom and Preset commands.
  The camera address can be specified.
  Config settings
      PTZDriver: "visca"
      PTZCameraAddress: 1
      PTZOutput: SEE BELOW

Pimoroni Pan-Tilt HAT Support
  Uses the Pimoroni Pan-Tilt HAT kit for the Raspberry Pi
  with Pan and Tilt functions
  Config settings
      PTZDriver: "pan-tilt-hat"

Waveshare Pan-Tilt HAT Support
  Uses the Waveshare Pan-Tilt HAT kit for the Raspberry Pi
  with Pan and Tilt functions
  Config settings
      PTZDriver: "TO BE ADDED"


RPOS ASCII Output Format
  RPOS has an internal ASCII string API generated by Pan/Tilt/Zoom/Preset and Imaging commands (eg brightness)
  These are passed out over the serial port or TCP connection
  Warning. This is the internal RPOS API which has been stable for some time, but may change with future ONVIF releases
  Config settings
      PTZDriver: "rposascii"

  Outputs are TAB separated values and end with LINE FEED (\n)
  TAB allows the use of spaces, commas and quotes in Preset Names
   
   gotohome
   sethome
   gotopreset     presetname   presetvalue
   setpreset      presetname   presetvalue
   clearpreset    presetname   presetvalue
   aux            auxname
   relayactive    relayname
   relayinactive  relayname
   ptz            panspeed   tiltspeed   zoomspeed
   absolute-ptz   panspeed   tiltspeed   zoomspeed
   releative-ptz  panspeed   tiltspeed   zoomspeed
   brightness     value
   focus          direction
   focusstop


PTZOutput
  The PTZ commands can be sent to a Serial Port or a TCP Socket
  Example 1
    Serial output, to a COM port with user defined baud rate and COM port settings
    PTZOutput: "serial"
    PTZSerialPort: "/dev/ttyUSB0"
    PTZSerialPortSettings" : { "baudRate":2400, "dataBits":8, "parity":"none", "stopBits":1 }

  Example 2
    TCP output, to a hostname and port
    PTZOutput: "tcp"
    PTZOutputURL: "127.0.0.1:9999"


ONVIF Imaging Service
  For Imaging Service commands (eg Brightness), commands are sent through to the V4L2 interface

*/

class ReconnectingStream extends events.EventEmitter {
  hostname: string;
  port: number|string;
  stream: any = null; // the currently open network socket

  constructor() {
    super();
  }

  reconnect() {
    console.log("PTZ Driver - Reconnecting after error");
    this.connect(this.hostname, this.port);
  }

  connect(hostname: string, port: number|string) {
    this.hostname = hostname;
    this.port = port;

    this.stream = new net.Socket();
    
    this.stream.on('connect', function() {
      console.log('PTZ Driver - Socket connected');
    })
    this.stream.on('data', (data) => {  
        console.log('PTZ Driver received socket data ' + data);
    });
    this.stream.on('close', () => {
      console.log('PTZ Driver - Socket closed');
      // schedule a reconnect in 1 second (TODO - add backoff strategy)
      setTimeout(() => {this.reconnect()}, 1000); // use => to make timeout function bind to this instance
    });
    this.stream.on('error', () => {
      console.log('PTZ Driver - Socket error');
    });
    this.stream.on('timeout', () => {
      console.log('PTZ Driver - Socket timeout error');
    });

    console.log('PTZ Driver connecting to ' + this.hostname + ':' + this.port);
    this.stream.connect(this.port, this.hostname, () => {
      console.log('PTZ Driver has connected to ' + this.hostname + ':' + this.port);
    });

  }


  write(data: string|Buffer) {
    // pass the Write through to the socket. If the socket is null, we throw away the data
    try {
      this.stream.write(data);
    } catch (err) {
      console.log('PTZ Driver - Data thrown away')
    }
  }
}
class PTZDriver {

  config: rposConfig;
  rposAscii: any;
  tenx: any;
  pelcod: any;
  visca: any;
  panTiltHat: any;
  serialPort: any;
  stream: any;
  supportsAbsolutePTZ: boolean = false;
  supportsRelativePTZ: boolean = false;
  supportsContinuousPTZ: boolean = false;
  supportsGoToHome: boolean = false;
  hasFixedHomePosition: boolean = true;

  constructor(config: rposConfig) {
    this.config = config;
    let parent = this;

    // Sanity checks. Do not open serial or socket if using USB Tenx driver
    let PTZOutput = config.PTZOutput;
    if (config.PTZDriver === 'tenx') {
      PTZOutput = 'none';
    }

    // Sanity checks. Do not open serial or socket if using Pan-Tilt HAT
    if (config.PTZDriver === 'pan-tilt-hat') {
      PTZOutput = 'none';
    }


    // Open the OUTPUT STREAM
    if (PTZOutput === 'serial') {
      var SerialPort = require('serialport');
      this.serialPort = new SerialPort(config.PTZSerialPort, 
        {
        baudRate: config.PTZSerialPortSettings.baudRate,
        parity:   config.PTZSerialPortSettings.parity,
        dataBits: config.PTZSerialPortSettings.dataBits,
        stopBits: config.PTZSerialPortSettings.stopBits,
        }
      );
 
      this.stream = this.serialPort.on("open", function(err){
          if (err) {
            console.log('Error: '+err);
            return;
          }
      });
    }

    if (PTZOutput === 'tcp') {
      let host = config.PTZOutputURL.split(':')[0];
      let port = config.PTZOutputURL.split(':')[1];      

      this.stream = new ReconnectingStream();
      this.stream.on('data', function(data) {  
          console.log('PTZ Driver received socket data ' + data);
      });

      console.log('PTZ Driver connecting to ' + host + ':' + port);

      this.stream.connect(host, port);
    }


    // Initialise specific drivers, map them onto a STREAM

    if (config.PTZDriver === 'tenx') {
      var TenxDriver = require('tenx-usb-missile-launcher-driver');
      this.tenx = new TenxDriver();
      this.tenx.open();
      this.supportsContinuousPTZ = true;
    }

    if (config.PTZDriver === 'rposascii') {
      this.rposAscii = true;
      this.supportsAbsolutePTZ = true;
      this.supportsRelativePTZ = true;
      this.supportsContinuousPTZ = true;
      this.supportsGoToHome = true;
    }

    if (config.PTZDriver === 'pan-tilt-hat') {
      var PanTiltHAT = require('pan-tilt-hat');
      this.panTiltHat = new PanTiltHAT();
      this.supportsAbsolutePTZ = true;
      this.supportsRelativePTZ = true;
      this.supportsContinuousPTZ = true;
      this.supportsGoToHome = true;
    }
    
    if (config.PTZDriver === 'pelcod') {
      var PelcoD = require('node-pelcod');
      this.pelcod = new PelcoD(this.stream);
      this.pelcod.setAddress(parent.config.PTZCameraAddress);
      this.supportsContinuousPTZ = true;
      this.supportsGoToHome = true;
      this.hasFixedHomePosition = false;
    }

    if (config.PTZDriver === 'visca') {
      this.visca = true;
      this.supportsContinuousPTZ = true;
      this.supportsGoToHome = true;
    }
  }


// Data messages have the following format
//   command
//   OR
//   command data
// 
//   Data can contain one or more of the following fields 
//     name:
//     value:
//     pan:
//     tilt:
//     zoom:
// JSON is used so that spaces and commas in names (eg Preset Names) is supported

// Use 'arrow functions' (instead of bind) to ensure the 'this' refers to the
// class and not to the caller's 'this'. This is required when process_ptz_command
// is used in a callback function.
  process_ptz_command = (command: string, data: any) => {
    if (command==='gotohome') {
      console.log("Goto Home");
      if (this.rposAscii) this.stream.write(command + '\n');
      if (this.pelcod) this.pelcod.sendGotoPreset(1); // use preset 1 for Home
      if (this.visca) {
        let data: number[] = [];
        data.push(0x81,0x01,0x06,0x04,0xff);
        this.stream.write(new Buffer(data));
      }
      if (this.panTiltHat) {
        this.panTiltHat.goto_home();
      }
    }
    else if (command==='sethome') {
      console.log("SetHome ");
      if (this.rposAscii) this.stream.write(command + '\n');
      if (this.pelcod) this.pelcod.sendSetPreset(1); // use preset 1 for Home
    }
    else if (command==='gotopreset') {
      console.log("Goto Preset "+ data.name + ' / ' + data.value);
      if (this.rposAscii) this.stream.write(command + '\t' + data.name + '\t' + data.value + '\n');
      if (this.tenx) this.tenx.fire();
      if (this.pelcod) this.pelcod.sendGotoPreset(parseInt(data.value));
    }
    else if (command==='setpreset') {
      console.log("Set Preset "+ data.name + ' / ' + data.value);
      if (this.rposAscii) this.stream.write(command + '\t' + data.name + '\t' + data.value + '\n');
      if (this.pelcod) this.pelcod.sendSetPreset(parseInt(data.value));
    }
    else if (command==='clearpreset') {
      console.log("Clear Preset "+ data.name + ' / ' + data.value);
      if (this.rposAscii) this.stream.write(command + '\t' + data.name + '\t' + data.value + '\n');
      if (this.pelcod) this.pelcod.sendClearPreset(parseInt(data.value));
    }
    else if (command==='aux') {
      console.log("Aux "+ data.name);
      if (this.rposAscii) this.stream.write(command + '\t' + data.name + '\n');
      if (this.pelcod) {
        if (data.name === 'AUX1on') this.pelcod.sendSetAux(1);
        if (data.name === 'AUX1off') this.pelcod.sendClearAux(1);
        if (data.name === 'AUX2on') this.pelcod.sendSetAux(2);
        if (data.name === 'AUX2off') this.pelcod.sendClearAux(2);
        if (data.name === 'AUX3on') this.pelcod.sendSetAux(3);
        if (data.name === 'AUX3off') this.pelcod.sendClearAux(3);
        if (data.name === 'AUX4on') this.pelcod.sendSetAux(4);
        if (data.name === 'AUX4off') this.pelcod.sendClearAux(4);
        if (data.name === 'AUX5on') this.pelcod.sendSetAux(5);
        if (data.name === 'AUX5off') this.pelcod.sendClearAux(5);
        if (data.name === 'AUX6on') this.pelcod.sendSetAux(6);
        if (data.name === 'AUX6off') this.pelcod.sendClearAux(6);
        if (data.name === 'AUX7on') this.pelcod.sendSetAux(7);
        if (data.name === 'AUX7off') this.pelcod.sendClearAux(7);
        if (data.name === 'AUX8on') this.pelcod.sendSetAux(8);
        if (data.name === 'AUX8off') this.pelcod.sendClearAux(8);
      }
    }
    else if (command==='relayactive') {
      console.log("Relay Active "+ data.name);
      if (this.rposAscii) this.stream.write(command + '\t' + data.name + '\n');
    }
    else if (command==='relayinactive') {
      console.log("Relay Inactive "+ data.name);
      if (this.rposAscii) this.stream.write(command + '\t' + data.name + '\n');
    }
    else if (command==='ptz') {
      console.log("Continuous PTZ "+ data.pan + '\t' + data.tilt + '\t' + data.zoom + '\n');
      var p=0.0;
      var t=0.0;
      var z=0.0;
      try {p = parseFloat(data.pan)} catch (err) {}
      try {t = parseFloat(data.tilt)} catch (err) {}
      try {z = parseFloat(data.zoom)} catch (err) {}
      if (this.rposAscii) this.stream.write(command + '\t' + p + '\t' + t + '\t' + z + '\n');
      if (this.tenx) {
        if      (p < -0.1 && t >  0.1) this.tenx.upleft();
        else if (p >  0.1 && t >  0.1) this.tenx.upright();
        else if (p < -0.1 && t < -0.1) this.tenx.downleft();
        else if (p >  0.1 && t < -0.1) this.tenx.downright();
        else if (p >  0.1) this.tenx.right();
        else if (p < -0.1) this.tenx.left();
        else if (t >  0.1) this.tenx.up();
        else if (t < -0.1) this.tenx.down()
        else this.tenx.stop();
      }
      if (this.pelcod) {
        this.pelcod.up(false).down(false).left(false).right(false);
        if      (p < 0 && t > 0) this.pelcod.up(true).left(true);
        else if (p > 0 && t > 0) this.pelcod.up(true).right(true);
        else if (p < 0 && t < 0) this.pelcod.down(true).left(true);
        else if (p > 0 && t < 0) this.pelcod.down(true).right(true);
        else if (p > 0) this.pelcod.right(true);
        else if (p < 0) this.pelcod.left(true);
        else if (t > 0) this.pelcod.up(true);
        else if (t < 0) this.pelcod.down(true);

        // Set Pan/Tilt speed
        // scale speeds from 0..1 to 0..63
        var pan_speed = Math.round(Math.abs(p) * 63.0 );
        var tilt_speed = Math.round(Math.abs(t) * 63.0 );

        this.pelcod.setPanSpeed(pan_speed);
        this.pelcod.setTiltSpeed(tilt_speed);


        this.pelcod.zoomIn(false).zoomOut(false);
        if (z>0) this.pelcod.zoomIn(true);
        if (z<0) this.pelcod.zoomOut(true);

        // Set Zoom speed
        // scale speeds from 0..1 to 0 (slow), 1 (low med), 2 (high med), 3 (fast)
        var abs_z = Math.abs(z);
        var zoom_speed = 0;
        if (abs_z > 0.75) zoom_speed = 3;
        else if (abs_z > 0.5) zoom_speed = 2;
        else if (abs_z > 0.25) zoom_speed = 1;
        else zoom_speed = 0;

        // sendSetZoomSpeed is not in node-pelcod yet so wrap with try/catch
        try {
          if (z != 0) this.pelcod.sendSetZoomSpeed(zoom_speed);
        } catch (err) {}

        this.pelcod.send();
      }
      if (this.visca) {
        // Map ONVIF Pan and Tilt Speed 0 to 1 to VISCA Speed 1 to 0x18
        // Map ONVIF Zoom Speed (0 to 1) to VISCA Speed 0 to 7
        let visca_pan_speed = ( Math.abs(p) * 0x18) / 1.0;
        let visca_tilt_speed = ( Math.abs(t) * 0x18) / 1.0;
        let visca_zoom_speed = ( Math.abs(z) * 0x07) / 1.0;

        // rounding check. Visca Pan/Tilt to be in range 0x01 .. 0x18
        if (visca_pan_speed === 0) visca_pan_speed = 1;
        if (visca_tilt_speed === 0) visca_tilt_speed = 1;

        if (this.config.PTZDriver === 'visca') {
          let data: number[] = [];
          if      (p < 0 && t > 0) { // upleft
            data.push(0x81,0x01,0x06,0x01,visca_pan_speed,visca_zoom_speed,0x01,0x01,0xff);
          }
          else if (p > 0 && t > 0) { // upright
            data.push(0x81,0x01,0x06,0x01,visca_pan_speed,visca_zoom_speed,0x02,0x01,0xff);
          }
          else if (p < 0 && t < 0) { // downleft;
            data.push(0x81,0x01,0x06,0x01,visca_pan_speed,visca_zoom_speed,0x01,0x02,0xff);
          }
          else if (p >  0 && t < 0) { // downright;
            data.push(0x81,0x01,0x06,0x01,visca_pan_speed,visca_zoom_speed,0x02,0x02,0xff);
          }
          else if (p > 0) { // right
            data.push(0x81,0x01,0x06,0x01,visca_pan_speed,0x00,0x02,0x03,0xff);
          }
          else if (p < 0) { // left
            data.push(0x81,0x01,0x06,0x01,visca_pan_speed,0x00,0x01,0x03,0xff);
          }
          else if (t > 0) { // up
            data.push(0x81,0x01,0x06,0x01,0x00,visca_tilt_speed,0x03,0x01,0xff);
          }
          else if (t < 0) { // down
            data.push(0x81,0x01,0x06,0x01,0x00,visca_tilt_speed,0x03,0x02,0xff);
          }
          else { // stop 
            data.push(0x81,0x01,0x06,0x01,0x00,0x00,0x03,0x03,0xff);
          }

          // Zoom
          if (z < 0) { // zoom out
            data.push(0x81,0x01,0x04,0x07,(0x30 + visca_zoom_speed),0xff);
          }
          else if (z > 0) { // zoom in
            data.push(0x81,0x01,0x04,0x07,(0x20 + visca_zoom_speed),0xff);
          } else { // zoom stop
            data.push(0x81,0x01,0x04,0x07,0x00,0xff);
          }

          this.stream.write(new Buffer(data));
        }
      }
      if (this.panTiltHat) {
        // Map ONVIF Pan and Tilt Speed 0 to 1 to Speed 0 to 15
        let pan_speed  = ( Math.abs(p) * 15) / 1.0;
        let tilt_speed = ( Math.abs(t) * 15) / 1.0;

        // rounding check.
        if (pan_speed > 15) pan_speed = 15;
        if (tilt_speed > 15) tilt_speed = 15;
        if (pan_speed < 0) pan_speed = 0;
        if (tilt_speed < 0) tilt_speed = 0;

        if (p < 0)  this.panTiltHat.pan_left(pan_speed);
        if (p > 0)  this.panTiltHat.pan_right(pan_speed);
        if (p == 0) this.panTiltHat.pan_right(0); // stop
        if (t < 0)  this.panTiltHat.tilt_down(tilt_speed);
        if (t > 0)  this.panTiltHat.tilt_up(tilt_speed);
        if (t == 0) this.panTiltHat.tilt_down(0); // stop
      }
    }
    else if (command==='absolute-ptz') {
      console.log("Absolute PTZ "+ data.pan + '\t' + data.tilt + '\t' + data.zoom);
      var p=0.0;
      var t=0.0;
      var z=0.0;
      try {p = parseFloat(data.pan)} catch (err) {}
      try {t = parseFloat(data.tilt)} catch (err) {}
      try {z = parseFloat(data.zoom)} catch (err) {}
      if (this.rposAscii) this.stream.write(command + '\t' + p + '\t' + t + '\t' + z + '\n');
      if (this.panTiltHat) {
          let new_pan_angle = p * 90.0
          this.panTiltHat.pan(Math.round(new_pan_angle));
          
          let new_tilt_angle = t * 80.0
          this.panTiltHat.tilt(Math.round(new_tilt_angle));
      }
    }
    else if (command==='relative-ptz') {
      console.log("Relative PTZ "+ data.pan + '\t' + data.tilt + '\t' + data.zoom);
      var p=0.0;
      var t=0.0;
      var z=0.0;
      try {p = parseFloat(data.pan)} catch (err) {}
      try {t = parseFloat(data.tilt)} catch (err) {}
      try {z = parseFloat(data.zoom)} catch (err) {}
      if (this.rposAscii) this.stream.write(command + '\t' + p + '\t' + t + '\t' + z + '\n');
      if (this.panTiltHat) {
          let pan_degrees = p * 90.0
          let new_pan_angle = this.panTiltHat.pan_position - pan_degrees;
          this.panTiltHat.pan(Math.round(new_pan_angle));
          
          let tilt_degrees = t * 80.0
          let new_tilt_angle = this.panTiltHat.tilt_position - tilt_degrees;
          this.panTiltHat.tilt(Math.round(new_tilt_angle));
      }
    }
    else if (command==='brightness') {
      console.log("Set Brightness "+ data.value);
      if (this.rposAscii) this.stream.write(command + '\t' + data.value + '\n');
      v4l2ctl.SetBrightness(data.value);
    }
    else if (command==='focus') {
      console.log("Focus "+ data.value);
      if (this.rposAscii) this.stream.write(command + '\t' + data.value + '\n');
      if (this.pelcod) {
        if (data.value < 0) this.pelcod.focusNear(true);
        else if (data.value > 0) this.pelcod.focusFar(true);
        else {
          this.pelcod.focusNear(false);
          this.pelcod.focusFar(false);
        }
        this.pelcod.send();
      }
    }
    else if (command==='focusstop') {
      console.log("Focus Stop");
      if (this.rposAscii) this.stream.write(command + '\n');
      if (this.pelcod) {
        this.pelcod.focusNear(false);
        this.pelcod.focusFar(false);
        this.pelcod.send();
      }
    }
    else {
      if (!data.value) {
        console.log("Unhandled PTZ/Imaging Command Received: " + command);
      } else {
        console.log("Unhandled PTZ/Imaging Command Received: " + command + ' Value:' + data.value);
      }
    }
  }
}

export = PTZDriver;
